
package frsf.cidisi.faia.kalman;

import frsf.cidisi.faia.tools.Matrix;
import frsf.cidisi.faia.tools.MatrixMathematics;
import frsf.cidisi.faia.tools.NoSquareException;

/**
 *
 * @author Chaca!!
 */
public class Kalman {
    
    /*
     * Observables y Escondidos...
     * por ejemplo: posicion y velocidad
     */
    
    int n; //tamaño ????
    
    Matrix vectorXPrima;    // x´ estimate
    Matrix vectorVarianzaPrima;  // P´ = uncertainty covariance
    Matrix vectorX;         // x actual
    Matrix vectorVarianza;  // P = uncertainty covariance
    Matrix stateTransition; // F = state transition matrix
    Matrix motionVector;    // u = motion vector
    Matrix measurement;     // z = measurement
    Matrix measurementFuction;  // H = measurement Fuction
    Matrix measurementNoise;    // R = measumentenNoise
    Matrix kalmanGain;      // K = ganancia de Kalman
    Matrix identity = Matrix.identity(n);
    
    /**
     * Temporales
     */
    Matrix y; // the error 
    Matrix S; // the error is mapped innto a matrix, "donde se proyecta la incertidumbre del sistema"

    /**
     * Genera Kalman, que contiene todas las variables para luego proceder en las operaciones
     * @param vectorX dimensiones, X, Y, velX, velY, angulo  (inicial)
     * @param vectorVarianza varianza de la posicion inicial
     * @param stateTransition F matriz de transición de estados
     * @param motionVector U vector con acciónes
     * @param measurement Z vector con datos de los sensores
     * @param measurementFuction H función de medición (a que variables afecta la medición)
     * @param measurementNoise  R ruido en la medición (incertidumbre de la mediciión)
     */
    public Kalman(Matrix vectorX, Matrix vectorVarianza, Matrix stateTransition, Matrix motionVector, Matrix measurement, Matrix measurementFuction, Matrix measurementNoise) {
        this.vectorX = vectorX;
        this.vectorVarianza = vectorVarianza;
        this.stateTransition = stateTransition;
        this.motionVector = motionVector;
        this.measurement = measurement;
        this.measurementFuction = measurementFuction;
        this.measurementNoise = measurementNoise;
        
        n = vectorVarianza.getNcols(); // asigno número de dimensiones a N
        this.identity = Matrix.identity(n);
    }
    
    
 
    
    
    
    
    /**
     * PASOS:
     * 1) x'= F.x + u
     * 2) P'=F.P.F(traspuesta)
     * 3) y = z - H.x
     * 4) S = H.P.H(traspuesta) + R
     * 5) K = P.H(traspuesta).S(inversa)
     * 6) x' = x + (K.y)
     * 7) P' = (I - K.H)P
     */
    
    /**
     * PREDICCION paso1 y paso2
     */
    
    public void paso1(){
    Matrix v1 = MatrixMathematics.multiply(stateTransition, vectorX);
    vectorXPrima = MatrixMathematics.sumar(v1,motionVector);
            }
    
    public void paso2(){
    Matrix v1 = MatrixMathematics.multiply(stateTransition, vectorVarianza);
    Matrix m1 = MatrixMathematics.transpose(stateTransition);
    vectorVarianzaPrima = MatrixMathematics.multiply(v1, m1);
    }
    
    /**
     *  MEASUREMENT paso 3, 4 y 5
     */
    
    public void paso3(){
        Matrix tmp = MatrixMathematics.multiply(measurementFuction, vectorXPrima);
        y = MatrixMathematics.restar(measurement, tmp);
    }
    
    public void paso4(){
        Matrix tmp1 = MatrixMathematics.multiply(measurementFuction, vectorVarianzaPrima);
        Matrix tmp2 = MatrixMathematics.multiply(tmp1,(MatrixMathematics.transpose(measurementFuction)));     
        S = MatrixMathematics.sumar(tmp2, measurementNoise);
    }
    
    public void paso5() throws NoSquareException{
        Matrix tmp1 = MatrixMathematics.multiply(vectorVarianzaPrima, MatrixMathematics.transpose(measurementFuction));
        kalmanGain = MatrixMathematics.multiply(tmp1, MatrixMathematics.inverse(S));
    }
    
    /**
     * UPDATE paso 6 y 7
     */    
  
    public void paso6(){        
        Matrix tmp1 = MatrixMathematics.multiply(kalmanGain, y);
        vectorX = MatrixMathematics.sumar(vectorXPrima, tmp1);

    }
    
    public void paso7(){        
        Matrix tmp1 = MatrixMathematics.multiply(kalmanGain, measurementFuction);
        Matrix tmp2 = MatrixMathematics.restar(identity, tmp1);     
        vectorVarianza = MatrixMathematics.multiply(tmp2, vectorVarianzaPrima);

    }

    
    
    
    
    
    public Matrix getS() {
        return S;
    }

    public void setS(Matrix S) {
        this.S = S;
    }

    public Matrix getIdentity() {
        return identity;
    }

    public void setIdentity(Matrix identity) {
        this.identity = identity;
    }

    public Matrix getKalmanGain() {
        return kalmanGain;
    }

    public void setKalmanGain(Matrix kalmanGain) {
        this.kalmanGain = kalmanGain;
    }

    public Matrix getMeasurement() {
        return measurement;
    }

    public void setMeasurement(Matrix measurement) {
        this.measurement = measurement;
    }

    public Matrix getMeasurementFuction() {
        return measurementFuction;
    }

    public void setMeasurementFuction(Matrix measurementFuction) {
        this.measurementFuction = measurementFuction;
    }

    public Matrix getMeasurementNoise() {
        return measurementNoise;
    }

    public void setMeasurementNoise(Matrix measurementNoise) {
        this.measurementNoise = measurementNoise;
    }

    public Matrix getMotionVector() {
        return motionVector;
    }

    public void setMotionVector(Matrix motionVector) {
        this.motionVector = motionVector;
    }

    public int getN() {
        return n;
    }

    public void setN(int n) {
        this.n = n;
    }

    public Matrix getStateTransition() {
        return stateTransition;
    }

    public void setStateTransition(Matrix stateTransition) {
        this.stateTransition = stateTransition;
    }

    public Matrix getVectorVarianza() {
        return vectorVarianza;
    }

    public void setVectorVarianza(Matrix vectorVarianza) {
        this.vectorVarianza = vectorVarianza;
    }

    public Matrix getVectorVarianzaPrima() {
        return vectorVarianzaPrima;
    }

    public void setVectorVarianzaPrima(Matrix vectorVarianzaPrima) {
        this.vectorVarianzaPrima = vectorVarianzaPrima;
    }

    public Matrix getVectorX() {
        return vectorX;
    }

    public void setVectorX(Matrix vectorX) {
        this.vectorX = vectorX;
    }

    public Matrix getVectorXPrima() {
        return vectorXPrima;
    }

    public void setVectorXPrima(Matrix vectorXPrima) {
        this.vectorXPrima = vectorXPrima;
    }

    public Matrix getY() {
        return y;
    }

    public void setY(Matrix y) {
        this.y = y;
    }
    
    
    
    
    
    
}
